/*
 * @Description: 
 * @Author: BiChunkai 321521004@qq.com
 * @Date: 2025-04-11 09:46:18
 * @FilePath: /ros2_test/src/camera_sync_cpp/src/camera_sync_node2.cpp
 * 
 * Copyright (c) 2025 by 无锡捷普迅科技有限公司, All Rights Reserved. 
 */
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

using namespace std::chrono_literals;

class CameraSyncNode : public rclcpp::Node
{
public:
    CameraSyncNode() : Node("camera_sync_node")
    {
        // 初始化cv窗口
        cv::namedWindow("RGB_Camera", cv::WINDOW_NORMAL);
        cv::namedWindow("Depth_Camera", cv::WINDOW_NORMAL);

        // 创建消息过滤器订阅者  
        // RGB_image_sub_.subscribe(this, "/camera/camera/color/image_raw");
        // Depth_image_sub_.subscribe(this, "/camera/camera/aligned_depth_to_color/image_raw");

        // RGB_image_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, "/camera/camera/color/image_raw");
        // Depth_image_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, "/camera/camera/aligned_depth_to_color/image_raw");

        RGB_image_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>();
        Depth_image_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>();
        RGB_image_sub_->subscribe(this, "/camera/camera/color/image_raw");
        Depth_image_sub_->subscribe(this, "/camera/camera/aligned_depth_to_color/image_raw");

        // 设置近似时间同步策略
        sync_ = std::make_shared<Sync>(ApproximateTimePolicy(10), *RGB_image_sub_, *Depth_image_sub_); // 10表示队列大小

        // 注册回调函数
        sync_->registerCallback(std::bind(&CameraSyncNode::image_callback, this, std::placeholders::_1, std::placeholders::_2));

        RCLCPP_INFO(this->get_logger(), "CameraSyncNode 启动！");
    }

    ~CameraSyncNode()
    {
        cv::destroyAllWindows();
    }

private:
    // 定义类型别名以简化代码
    using ApproximateTimePolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image>;
    using Sync = message_filters::Synchronizer<ApproximateTimePolicy>;
    // 创建同步器
    std::shared_ptr<Sync> sync_; // 指向Sync的智能指针，负责处理时间同步

    // 定义回调函数
    void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr &RGB_image_msg,
                        const sensor_msgs::msg::Image::ConstSharedPtr &Depth_image_msg)
    {
        try
        {
            // 将ROS消息转换为OpenCV格式

            const uint8_t *data = RGB_image_msg->data.data();
            cv::Mat RGB_image(RGB_image_msg->height, RGB_image_msg->width, CV_8UC3, const_cast<uint8_t *>(data));
            cv::cvtColor(RGB_image, RGB_image, cv::COLOR_RGB2BGR);

            cv_bridge::CvImagePtr cv_ptr;
            if (Depth_image_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
            {
                cv_ptr = cv_bridge::toCvCopy(Depth_image_msg, sensor_msgs::image_encodings::TYPE_16UC1);
            }
            else
            {
                cv_ptr = cv_bridge::toCvCopy(Depth_image_msg, sensor_msgs::image_encodings::TYPE_32FC1);
            }

            cv::Mat Depth_image = cv_ptr->image;

            // cv::Mat RGB_image = cv_bridge::toCvShare(RGB_image_msg, "bgr8")->image;
            // cv::Mat Depth_image = cv_bridge::toCvShare(Depth_image_msg, "mono16")->image;

            // 显示同步后的图像
            cv::imshow("RGB_Camera", RGB_image);
            cv::imshow("Depth_Camera", Depth_image);
            cv::waitKey(1);
        }
        catch (const cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "CV Bridge error: %s", e.what());
        }
    }

    // 定义消息过滤器订阅者
    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > RGB_image_sub_;
    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > Depth_image_sub_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CameraSyncNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
